#include "./PidCotroler.h"

PidController::PidController(float kp, float ki, float kd)
    : _kp(kp), _ki(ki), _kd(kd),
      _integral(0.0f), _previousError(0.0f),
      _outputMin(0.0f), _outputMax(1000.0f) {}

void PidController::setTunings(float kp, float ki, float kd) {
    _kp = kp;
    _ki = ki;
    _kd = kd;
}

void PidController::setOutputLimits(float min, float max) {
    _outputMin = min;
    _outputMax = max;
}

void PidController::reset() {
    _integral = 0.0f;
    _previousError = 0.0f;
}

float PidController::compute(float target, float measurement) {
    float error = target - measurement;

    _integral += error;
    float derivative = error - _previousError;

    float output = _kp * error + _ki * _integral + _kd * derivative;

    // 限幅
    if (output > _outputMax) output = _outputMax;
    else if (output < _outputMin) output = _outputMin;

    _previousError = error;
    return output;
}
